classdef SLIP < handle
    %SLIP
    %   Version 1.2H Feb. 18, 2013
    %   class for basic spring mass dynamics, provides integration of
    %   equation of motion for hopping, running and walking
    %
    %   used with analytically determined values for leg length and
    %   stiffness for human walking
        
    properties
        param;
        state;
        env;
        time;
        props;
        sim;
        model;
    end
    
    properties(SetAccess='private')
        
    end
    
    methods                                 %public methods
        %% Constructor
        function obj = SLIP()
            obj.param.k = 15000;
            obj.param.g = 9.81;
            obj.param.m = 80;
            obj.param.l0 = 1;
            obj.param.aTD = 68;
            obj.state.IC = [0 0 0.95 0];
            obj.state.xe = obj.state.IC;
            obj.sim.ie = 1;
            obj.time.te = 0;
            obj.model.reltol=1e-9;
            obj.model.abstol=1e-6;
            obj.time.xe=[];
            obj.time.te=0;
            obj.time.t=[];
            obj.state.X=[];
            obj.model.fast=1;
            obj.sim.error=0;
            obj.env.FP=[0 0 NaN];
            obj.time.timer=[];
            obj.model.stanceleg=1;
            obj.sim.vis=0;
            obj.model.walk_phases=2;
            obj.sim.transition=1;
        end
        
        %% SET functions
        % set initial conditions
        function obj = setprops(obj)
            propertyGUI(obj);
            obj=setIC(obj,obj.state.IC);
            obj=SLIPreset(obj);
        end
        
        function obj = SLIPreset(obj)
            obj.state.X=[];
            obj.state.leg=[];
            obj.state.lphi=[];
            obj.state.F=[];
            obj.state.E=[];
            obj.time.t=[];
            obj.props.l_left=[];
            obj.props.l_right=[];
            obj.props.E=[];
            obj.sim.transition=1;
            obj = reset_error(obj);
        end
        
        function obj = initFP(obj)
            obj.env.FP=[obj.state.IC(1) 0 0];
        end
        
        function obj = setIC(obj,init)
            assert(length(init)==4,'Initial condition requires a 4x1 vector')
            obj.state.IC=init;
            obj.state.xe=init;
            obj.time.te=0;
            obj.time.curr_phase=1;
            obj = reset_error(obj);
        end
        
        function obj = setstartleg(obj,leg)
            assert(ismember(leg,{'left' 'right'}),'input should be "left" (default, 1) or "right"')
            obj.model.stanceleg=strcmp(leg,'left');
        end
        
        function obj = setwalkphases(obj,phases)
            assert(ismember(phases,[2 4]),'Number of phases can be 2 or 4')
            obj.model.walk_phases=phases;
        end
        
        function obj = setcurrphase(obj,phase)
            obj.time.curr_phase=phase;
        end
        
        function obj = reset_error(obj)
            obj.sim.error=0;
            obj.sim.ie=[];
        end
        
        %% call motion phases
        % call flight phase integration
        function obj = int_hop_flight(obj)
            obj.model.options = odeset('RelTol', obj.model.reltol, 'AbsTol', obj.model.abstol, 'Events', @obj.hop_flight_events);
            obj.model.state_fun = @obj.flight;
            obj = SLIPintegrate(obj);
        end
        
        %call stance phase integration
        function obj = int_hop_stance(obj)
            obj.model.options = odeset('RelTol', obj.model.reltol, 'AbsTol', obj.model.abstol, 'Events', @obj.hop_stance_events);
            obj.model.state_fun = @obj.single_support;
            obj = SLIPintegrate(obj);
        end
        
        %call flight phase for running
        function obj = int_run_flight(obj)
            obj.model.options = odeset('RelTol', obj.model.reltol, 'AbsTol', obj.model.abstol, 'Events', @obj.run_flight_events);
            obj.model.state_fun = @obj.flight;
            obj = SLIPintegrate(obj);
            if ~obj.sim.error
                obj.env.FP = [obj.env.FP;obj.state.X(end,1)+obj.param.l0*cosd(obj.param.aTD) obj.state.X(end,3)-obj.param.l0*sind(obj.param.aTD) obj.time.te];
            end
            %visualisation
            if obj.sim.vis
                plot(obj.state.X(:,1),obj.state.X(:,3))
                hold on
                plot([obj.env.FP(end,1) obj.state.X(end,1)],[obj.env.FP(end,2) obj.state.X(end,3)],'k')
                plot([obj.state.X(end,1) obj.env.FP(end,1)],[obj.state.X(end,3) obj.env.FP(end,2)],'k*')
                grid on
                axis equal
                if obj.sim.error
                    disp(['simulation error! stopping simulation. Error code: ',num2str(obj.sim.error)]);
                end
            end
        end
        
        function obj = int_run_flight_ap(obj)
            obj.model.options = odeset('RelTol', obj.model.reltol, 'AbsTol', obj.model.abstol, 'Events', @obj.run_flight_events_ap);
            obj.model.state_fun = @obj.flight;
            obj = SLIPintegrate(obj);
            if obj.sim.error==0
                try
                    obj.env.FP = [obj.env.FP;obj.state(end,1)+obj.param.l0*cosd(obj.param.aTD) obj.state(end,3)-obj.param.l0*sind(obj.param.aTD) obj.time.te(1)];
                end
            end
            %visualisation
            if obj.sim.vis
                plot(obj.state.X(:,1),obj.state.X(:,3))
                hold on
                plot([obj.env.FP(end-1,1) obj.state.X(1,1)],[obj.env.FP(end-1,2) obj.state.X(1,3)],'k')
                plot([obj.env.FP(end-1,1) obj.state.X(1,1)],[obj.env.FP(end-1,2) obj.state.X(1,3)],'k*')
                grid on
                axis equal
                if obj.sim.error
                    disp(['simulation error! stopping simulation. Error code: ',num2str(obj.sim.error)]);
                end
            end
        end
        
        %call stance phase for running
        function obj = int_run_stance(obj)
            obj.model.options = odeset('RelTol', obj.model.reltol, 'AbsTol', obj.model.abstol, 'Events', @obj.run_stance_events);
            obj.model.state_fun = @obj.single_support;
            obj = SLIPintegrate(obj);
            %visualisation
            if obj.sim.vis
                plot(obj.state.X(:,1),obj.state.X(:,3))
                hold on
                plot([obj.env.FP(end,1) obj.state.X(end,1)],[obj.env.FP(end,2) obj.state.X(end,3)],'k')
                plot([obj.state.X(end,1) obj.env.FP(end,1)],[obj.state.X(end,3) obj.env.FP(end,2)],'k*')
                grid on
                axis equal
                if obj.sim.error
                    disp(['simulation error! stopping simulation. Error code: ',num2str(obj.sim.error)]);
                end
            end
        end
        
        % call single support integration
        function obj = int_walk_single(obj)
            %determine current phase to call
            if obj.model.walk_phases==2
                obj.model.options = odeset('RelTol',obj.model.reltol, 'AbsTol', obj.model.abstol, 'Events', @obj.walk_single_events);
            else
                if obj.time.curr_phase==1
                    obj.model.options = odeset('RelTol', obj.model.reltol, 'AbsTol', obj.model.abstol, 'Events', @obj.walk_single_events);
                else
                    obj.model.options = odeset('RelTol', obj.model.reltol, 'AbsTol', obj.model.abstol, 'Events', @obj.walk_single2VLO_events);
                end
            end
            %call fast or slow integration
            obj.model.state_fun = @obj.single_support;
            %integrate
            obj = SLIPintegrate(obj);
            %visualisation
            if obj.sim.vis
                plot(obj.state.X(:,1),obj.state.X(:,3))
                hold on
                plot([obj.state.X(end,1) obj.env.FP(end,1)],[obj.state.X(end,3) obj.env.FP(end,2)],'k')
                plot([obj.state.X(end,1) obj.env.FP(end,1)],[obj.state.X(end,3) obj.env.FP(end,2)],'k*')
                grid on
                plot(cos(0:0.01:pi/2),sin(0:0.01:pi/2),'r')
                axis equal
%                 if obj.sim.error
%                     disp(['simulation error! stopping simulation. Error code: ',num2str(obj.sim.error)]);
%                 end
            end
            if ~obj.sim.error
                switch obj.time.curr_phase
                    case 1,
                        try
                            obj.env.FP=[obj.env.FP;obj.state.X(end,1)+obj.param.l0*cosd(obj.param.aTD) obj.state.X(end,3)-obj.param.l0*sind(obj.param.aTD) obj.time.te];
                        catch me
                            me;
                        end
                        obj.model.stanceleg=~obj.model.stanceleg;
                        obj.time.curr_phase=obj.time.curr_phase+1;
                    case 4,
                        obj.time.curr_phase=1;
                    otherwise,
                        obj.time.curr_phase=obj.time.curr_phase+1;
                end
            end
        end
        
        
        % call double support integration
        function obj = int_walk_double(obj)
            if obj.model.walk_phases==2
                obj.model.options = odeset('RelTol', obj.model.reltol, 'AbsTol', obj.model.abstol, 'Events', @obj.walk_double_events);
            else
                if obj.time.curr_phase==3
                    obj.model.options = odeset('RelTol', obj.model.reltol, 'AbsTol', obj.model.abstol, 'Events', @obj.walk_double_events);
                else
                    obj.model.options = odeset('RelTol', obj.model.reltol, 'AbsTol', obj.model.abstol, 'Events', @obj.walk_double2ms_events);
                end
            end
            obj.model.state_fun = @obj.double_support;
            obj = SLIPintegrate(obj);
            if obj.model.walk_phases==2
                obj.time.curr_phase=1;
            else
                obj.time.curr_phase=obj.time.curr_phase+1;
            end
            if obj.sim.vis
                plot(obj.state.X(:,1),obj.state.X(:,3))
                hold on
                plot([obj.env.FP(end-1,1) obj.state.X(obj.sim.transition(end-1),1) obj.env.FP(end,1)],[obj.env.FP(end-1,2) obj.state.X(obj.sim.transition(end-1),3) obj.env.FP(end,2)],'k')
                plot([obj.env.FP(end-1,1) obj.state.X(obj.sim.transition(end-1),1) obj.env.FP(end,1)],[obj.env.FP(end-1,2) obj.state.X(obj.sim.transition(end-1),3) obj.env.FP(end,2)],'k*')
                plot([obj.env.FP(end-1,1) obj.state.X(end,1) obj.env.FP(end,1)],[obj.env.FP(end-1,2) obj.state.X(end,3) obj.env.FP(end,2)],'k')
                plot([obj.env.FP(end-1,1) obj.state.X(end,1) obj.env.FP(end,1)],[obj.env.FP(end-1,2) obj.state.X(end,3) obj.env.FP(end,2)],'k*')
                grid on
                axis equal
                if obj.sim.error
                    disp(['simulation error! stopping simulation. Error code: ',num2str(obj.sim.error)]);
                end
            end
        end
        
    end
    
    methods (Access='private')
        
        
        %% Integrator
        function obj = SLIPintegrate(obj)
            [t, x, obj.time.te, obj.state.xe, obj.sim.ie] = ode45(obj.model.state_fun,...
                obj.time.te:0.001:obj.time.te+3, obj.state.xe, obj.model.options);
            obj.time.t = [obj.time.t;t];
            obj.state.X = [obj.state.X;x];
            %error handling
            obj = error_handling(obj);
            if ~obj.sim.error
                obj.sim.transition=[obj.sim.transition;obj.sim.transition(end)+length(t)];
            end
        end
            
        
        %% EOM for motion phases
        
        %EOM flight phase
        function xdot = flight(obj,~,x)
            xdot = [x(2) 0 x(4) -obj.param.g]';
        end
        
        %% single support EOM
        
        %EOM single support
        function xdot = single_support(obj,~,x)
            L = sqrt( (x(1)-obj.env.FP(end,1))^2 + (x(3)-obj.env.FP(end,2))^2 );
            xdot = [x(2); obj.param.k/obj.param.m*(obj.param.l0-L)*(x(1)-obj.env.FP(end,1))/L; x(4); obj.param.k/obj.param.m*(obj.param.l0-L)*(x(3)-obj.env.FP(end,2))/L-obj.param.g];
        end
        
        %% double support EOM
        
        %EOM double support
       
        function xdot = double_support(obj,~,x)
            xt1 = obj.env.FP(end-1,1);
            yt1 = obj.env.FP(end-1,2);
            xt2 = obj.env.FP(end,1);
            yt2 = obj.env.FP(end,2);
            L1 = sqrt( (x(1)-xt1)^2 + (x(3)-yt1)^2 );
            L2 = sqrt( (x(1)-xt2)^2 + (x(3)-yt2)^2 );
            xdot = [x(2);...
                obj.param.k/obj.param.m*(obj.param.l0-L1)*(x(1)-xt1)/L1+obj.param.k/obj.param.m*(obj.param.l0-L2)*(x(1)-xt2)/L2;...
                x(4);...
                obj.param.k/obj.param.m*(obj.param.l0-L1)*(x(3)-yt1)/L1+obj.param.k/obj.param.m*(obj.param.l0-L2)*(x(3)-yt2)/L2-obj.param.g];
        end
        
        %% Hopping spring mass model
        
        %event handler flight phase hopping
        function [value, isterminal, direction] = hop_flight_events(obj, ~, x)
            value = obj.param.l0-x(3);     % detect TD height
            isterminal = 1;          % stop the integration
            direction = 1;          % negative direction
        end
        
        %event handler stance phase hopping
        function [value, isterminal, direction] = hop_stance_events(obj, ~, x)
            value = obj.param.l0-x(3);     % detect TO height
            isterminal = 1;          % stop the integration
            direction = -1;           % positive direction
        end
        
        %% running simulation
        
        function [value, isterminal, direction] = run_flight_events(obj, ~, x)
            value = x(3) - obj.param.l0 * sind (obj.param.aTD);
            isterminal = 1;
            direction = -1;
        end
        
        function [value, isterminal, direction] = run_flight_events_ap(~, ~, x)
            value = x(4);
            isterminal = 1;
            direction = -1;
        end
        
        function [value, isterminal, direction] = run_stance_events(obj, ~, x)
            value = [obj.param.l0 - sqrt((x(1) - obj.env.FP(end,1))^2 + (x(3) - obj.env.FP(end,2))^2)...
                x(3)...
                x(2)+1e-3];
            isterminal = [1 1 1];
            direction = [-1 -1 -1];
        end
        
        %% walking simulation
        
        %event handler single support phase walking
        function [value, isterminal, direction] = walk_single_events(obj, ~, x)
            value = [x(3)-obj.param.l0*sind(obj.param.aTD)...                                   % TD condition
                x(3)...                                                             % vertical position (fall)
                x(2)+1e-3...                                                        % horizontal speed
                obj.param.l0-sqrt( (x(1)-obj.env.FP(end,1))^2 + (x(3)-obj.env.FP(end,2))^2 )];    % stance leg looses contact
            isterminal = [1 1 1 1];          % stop the integration
            direction = [-1 -1 -1 -1];       % negative direction
        end
        
        %event handler single support phase walking trigger VLO
        function [value, isterminal, direction] = walk_single2VLO_events(obj, ~, x)
            value = [x(1)-obj.env.FP(end,1)...                                          % VLO
                x(3)...                                                             % vertical position
                x(2)+1e-3...                                                        % horizontal speed
                obj.param.l0-sqrt( (x(1)-obj.env.FP(end,1))^2 + (x(3)-obj.env.FP(end,2))^2 )];    % stance leg looses contact
            isterminal = [1 1 1 1];          % stop the integration
            direction = [0 -1 -1 -1];           % negative direction
        end
        
        %event handler double support phase walking
        function [value, isterminal, direction] = walk_double_events(obj, ~, x)
            value = [obj.param.l0-sqrt( (x(1)-obj.env.FP(end-1,1))^2 + (x(3) - obj.env.FP(end-1,2))^2)...  % TO
                x(3)...                                                                      % vertical position
                x(2)+1e-3...                                                                 % horizontal velocity
                obj.param.l0-sqrt( (x(1)-obj.env.FP(end,1))^2 + (x(3)-obj.env.FP(end,2))^2 )...            % leading leg looses contact
                x(1)+(1e-5)-obj.env.FP(end,1)];                                                  % close to VLO and still in double support
            isterminal = [1 1 1 1 1];          % stop the integration
            direction = [-1 -1 -1 -1 1];          % negative direction
        end
        
        %event handler LTP double support phase walking
        function [value, isterminal, direction] = walk_double2ms_events(obj, ~, x)
            value = [x(4)...                                                                % LTP
                x(3)...                                                                     % vertical position
                x(2)+1e-3...                                                                % horizontal speed
                obj.param.l0-sqrt( (x(1)-obj.env.FP(end,1))^2 + (x(3)-obj.env.FP(end,2))^2 )];            % leading leg looses contact
            isterminal = [1 1 1 1];          % stop the integration
            direction = [1 -1 -1 -1];          % negative direction
        end
        
        %% integration service functions
        
        %error handling
        function obj = error_handling(obj)
            if sum(obj.sim.ie)==1 && obj.sim.error==0
                obj.sim.error=0;
            else
                if obj.sim.error == 0
                    if ~isempty(obj.sim.ie)
                        obj.sim.error = obj.sim.ie;
                    else
                        obj.sim.error=9;
                    end
                else
                    obj.sim.error = obj.sim.error;
                end
            end
        end
        
    end
end
